
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mp_pitch_controller.c
  * @author     baiyang
  * @date       2023-6-30
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "mp_pitch_controller.h"

#include <ahrs/ahrs_view.h>
#include <parameter/param.h>
#include <gcs_mavlink/gcs.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static float _get_rate_out(mp_pitch_controller_t pitch_ctrl, float desired_rate, float scaler, bool disable_integrator, float aspeed, bool ground_mode);
static float _get_coordination_rate_offset(mp_pitch_controller_const_t pitch_ctrl, float *aspeed, bool *inverted);
static void pctrl_assign_param(mp_pitch_controller_t pitch_ctrl);
/*----------------------------------variable----------------------------------*/
/* define parameters */
static param_t var_info[] = {
    // @Param: 2SRV_TCONST
    // @DisplayName: Pitch Time Constant
    // @Description: Time constant in seconds from demanded to achieved pitch angle. Most models respond well to 0.5. May be reduced for faster responses, but setting lower than a model can achieve will not help.
    // @Range: 0.4 1.0
    // @Units: s
    // @Increment: 0.1
    // @User: Advanced
    PARAM_DEFINE_FLOAT(PTCH2SRV_TCONST,0.5f),

    // @Param: 2SRV_RMAX_UP
    // @DisplayName: Pitch up max rate
    // @Description: This sets the maximum nose up pitch rate that the attitude controller will demand (degrees/sec) in angle stabilized modes. Setting it to zero disables the limit.
    // @Range: 0 100
    // @Units: deg/s
    // @Increment: 1
    // @User: Advanced
    PARAM_DEFINE_INT16(PTCH2SRV_RMAX_UP,0),

    // @Param: 2SRV_RMAX_DN
    // @DisplayName: Pitch down max rate
    // @Description: This sets the maximum nose down pitch rate that the attitude controller will demand (degrees/sec) in angle stabilized modes. Setting it to zero disables the limit.
    // @Range: 0 100
    // @Units: deg/s
    // @Increment: 1
    // @User: Advanced
    PARAM_DEFINE_INT16(PTCH2SRV_RMAX_DN,0),

    // @Param: 2SRV_RLL
    // @DisplayName: Roll compensation
    // @Description: Gain added to pitch to keep aircraft from descending or ascending in turns. Increase in increments of 0.05 to reduce altitude loss. Decrease for altitude gain.
    // @Range: 0.7 1.5
    // @Increment: 0.05
    // @User: Standard
    PARAM_DEFINE_FLOAT(PTCH2SRV_RLL,1.0f),

    PARAM_DEFINE_FLOAT(PTCH_RATE_P,0.04f),
    PARAM_DEFINE_FLOAT(PTCH_RATE_I,0.15f),
    PARAM_DEFINE_FLOAT(PTCH_RATE_D,0),
    PARAM_DEFINE_FLOAT(PTCH_RATE_FF,0.345f),
    PARAM_DEFINE_FLOAT(PTCH_RATE_IMAX,0.666f),
    PARAM_DEFINE_FLOAT(PTCH_RATE_FLTT,3.0f),
    PARAM_DEFINE_FLOAT(PTCH_RATE_FLTE,0.0f),
    PARAM_DEFINE_FLOAT(PTCH_RATE_FLTD,12.0f),
    PARAM_DEFINE_FLOAT(PTCH_RATE_SMAX,150),
};

PARAM_GROUP_DEFINE(PTCH_CTRL, var_info);

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void pctrl_ctor(mp_pitch_controller_t pitch_ctrl, mb_fixed_wing_const_t parms, float dt)
{
    pitch_ctrl->aparm = parms;
    pitch_ctrl->_dt = dt;

    pid_ctrl_ctor2(&pitch_ctrl->rate_pid, 0.04f, 0.15f, 0, 0.345f, 0.666f, 3, 0, 12, 150, 1, dt);
    pid_ctrl_set_slew_limit_scale(&pitch_ctrl->rate_pid, 45);

    pctrl_assign_param(pitch_ctrl);
}

/*
 Function returns an equivalent elevator deflection in centi-degrees in the range from -4500 to 4500
 A positive demand is up
 Inputs are:
 1) demanded pitch rate in degrees/second
 2) control gain scaler = scaling_speed / aspeed
 3) boolean which is true when stabilise mode is active
 4) minimum FBW airspeed (metres/sec)
 5) maximum FBW airspeed (metres/sec)
*/
float pctrl_get_rate_out(mp_pitch_controller_t pitch_ctrl, float desired_rate, float scaler)
{
    float aspeed;
    if (!ahrs_airspeed_estimate(get_ahrs_view(), &aspeed)) {
        // If no airspeed available use average of min and max
        aspeed = 0.5f*(pitch_ctrl->aparm->airspeed_min + pitch_ctrl->aparm->airspeed_max);
    }
    return _get_rate_out(pitch_ctrl, desired_rate, scaler, false, aspeed, false);
}

// Function returns an equivalent elevator deflection in centi-degrees in the range from -4500 to 4500
// A positive demand is up
// Inputs are:
// 1) demanded pitch angle in centi-degrees
// 2) control gain scaler = scaling_speed / aspeed
// 3) boolean which is true when stabilise mode is active
// 4) minimum FBW airspeed (metres/sec)
// 5) maximum FBW airspeed (metres/sec)
//
float pctrl_get_servo_out(mp_pitch_controller_t pitch_ctrl, int32_t angle_err, float scaler, bool disable_integrator, bool ground_mode)
{
    // Calculate offset to pitch rate demand required to maintain pitch angle whilst banking
    // Calculate ideal turn rate from bank angle and airspeed assuming a level coordinated turn
    // Pitch rate offset is the component of turn rate about the pitch axis
    float aspeed;
    float rate_offset;
    bool inverted;

    if (pitch_ctrl->gains.tau < 0.05f) {
        pitch_ctrl->gains.tau = 0.05f;
    }

    rate_offset = _get_coordination_rate_offset(pitch_ctrl, &aspeed, &inverted);

    // Calculate the desired pitch rate (deg/sec) from the angle error
    pitch_ctrl->angle_err_deg = angle_err * 0.01;
    float desired_rate = pitch_ctrl->angle_err_deg / pitch_ctrl->gains.tau;

    // limit the maximum pitch rate demand. Don't apply when inverted
    // as the rates will be tuned when upright, and it is common that
    // much higher rates are needed inverted
    if (!inverted) {
        desired_rate += rate_offset;
        if (pitch_ctrl->gains.rmax_neg && desired_rate < -pitch_ctrl->gains.rmax_neg) {
            desired_rate = -pitch_ctrl->gains.rmax_neg;
        } else if (pitch_ctrl->gains.rmax_pos && desired_rate > pitch_ctrl->gains.rmax_pos) {
            desired_rate = pitch_ctrl->gains.rmax_pos;
        }
    } else {
        // Make sure not to invert the turn coordination offset
        desired_rate = -desired_rate + rate_offset;
    }

    /*
      when we are past the users defined roll limit for the aircraft
      our priority should be to bring the aircraft back within the
      roll limit. Using elevator for pitch control at large roll
      angles is ineffective, and can be counter productive as it
      induces earth-frame yaw which can reduce the ability to roll. We
      linearly reduce pitch demanded rate when beyond the configured
      roll limit, reducing to zero at 90 degrees
    */
    ahrs_view *_ahrs = get_ahrs_view();
    float roll_wrapped = labs(_ahrs->roll_sensor_cd);
    if (roll_wrapped > 9000) {
        roll_wrapped = 18000 - roll_wrapped;
    }
    const float roll_limit_margin = MIN(pitch_ctrl->aparm->roll_limit_cd + 500.0f, 8500.0f);
    if (roll_wrapped > roll_limit_margin && labs(_ahrs->pitch_sensor_cd) < 7000) {
        float roll_prop = (roll_wrapped - roll_limit_margin) / (float)(9000 - roll_limit_margin);
        desired_rate *= (1 - roll_prop);
    }

    return _get_rate_out(pitch_ctrl, desired_rate, scaler, disable_integrator, aspeed, ground_mode);
}

/*
  start an autotune
 */
void pctrl_autotune_start(mp_pitch_controller_t pitch_ctrl)
{
    if (pitch_ctrl->autotune == NULL) {
        pitch_ctrl->autotune = mp_autotune_probe(&pitch_ctrl->gains, MP_AUTOTUNE_PITCH, pitch_ctrl->aparm, &pitch_ctrl->rate_pid, pitch_ctrl->_dt);
        if (pitch_ctrl->autotune == NULL) {
            if (!pitch_ctrl->failed_autotune_alloc) {
                GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "AutoTune: failed roll allocation");
            }
            pitch_ctrl->failed_autotune_alloc = true;
        }
    }
    if (pitch_ctrl->autotune != NULL) {
        mp_autotune_start(pitch_ctrl->autotune);
    }
}

/*
  restore autotune gains
 */
void pctrl_autotune_restore(mp_pitch_controller_t pitch_ctrl)
{
    if (pitch_ctrl->autotune != NULL) {
        mp_autotune_stop(pitch_ctrl->autotune);
    }
}

/*
  AC_PID based rate controller
*/
static float _get_rate_out(mp_pitch_controller_t pitch_ctrl, float desired_rate, float scaler, bool disable_integrator, float aspeed, bool ground_mode)
{
    ahrs_view *_ahrs = get_ahrs_view();

    const float eas2tas = ahrs_get_EAS2TAS(_ahrs);
    bool limit_I = fabsf(pitch_ctrl->_last_out) >= 45;
    float rate_y = _ahrs->gyr.y;
    float old_I = pid_ctrl_get_i(&pitch_ctrl->rate_pid);

    bool underspeed = aspeed <= 0.5f*(float)(pitch_ctrl->aparm->airspeed_min);
    if (underspeed) {
        limit_I = true;
    }

    // the P and I elements are scaled by sq(scaler). To use an
    // unmodified AC_PID object we scale the inputs and calculate FF separately
    //
    // note that we run AC_PID in radians so that the normal scaling
    // range for IMAX in AC_PID applies (usually an IMAX value less than 1.0)
    pid_ctrl_update_all(&pitch_ctrl->rate_pid, radians(desired_rate) * scaler * scaler, rate_y * scaler * scaler, limit_I);

    if (underspeed) {
        // when underspeed we lock the integrator
        pid_ctrl_set_integrator1(&pitch_ctrl->rate_pid, old_I);
    }

    // FF should be scaled by scaler/eas2tas, but since we have scaled
    // the AC_PID target above by scaler*scaler we need to instead
    // divide by scaler*eas2tas to get the right scaling
    const float ff = degrees(pid_ctrl_get_ff(&pitch_ctrl->rate_pid) / (scaler * eas2tas));

    if (disable_integrator) {
        pid_ctrl_reset_i(&pitch_ctrl->rate_pid);
    }

    // convert AC_PID info object to same scale as old controller
    pitch_ctrl->_pid_info = pitch_ctrl->rate_pid._pid_info;

    const float deg_scale = degrees(1);
    pitch_ctrl->_pid_info.FF = ff;
    pitch_ctrl->_pid_info.P *= deg_scale;
    pitch_ctrl->_pid_info.I *= deg_scale;
    pitch_ctrl->_pid_info.D *= deg_scale;

    // fix the logged target and actual values to not have the scalers applied
    pitch_ctrl->_pid_info.target = desired_rate;
    pitch_ctrl->_pid_info.actual = degrees(rate_y);

    // sum components
    float out = pitch_ctrl->_pid_info.FF + pitch_ctrl->_pid_info.P + pitch_ctrl->_pid_info.I + pitch_ctrl->_pid_info.D;
    if (ground_mode) {
        // when on ground suppress D and half P term to prevent oscillations
        out -= pitch_ctrl->_pid_info.D + 0.5f*pitch_ctrl->_pid_info.P;
    }

    // remember the last output to trigger the I limit
    pitch_ctrl->_last_out = out;

    if (pitch_ctrl->autotune != NULL && pitch_ctrl->autotune->running && aspeed > pitch_ctrl->aparm->airspeed_min) {
        // let autotune have a go at the values
        mp_autotune_update(pitch_ctrl->autotune, &pitch_ctrl->_pid_info, scaler, pitch_ctrl->angle_err_deg);
    }

    // output is scaled to notional centidegrees of deflection
    return math_constrain_float(out * 100, -4500, 4500);
}

/*
  get the rate offset in degrees/second needed for pitch in body frame
  to maintain height in a coordinated turn.

  Also returns the inverted flag and the estimated airspeed in m/s for
  use by the rest of the pitch controller
 */
static float _get_coordination_rate_offset(mp_pitch_controller_const_t pitch_ctrl, float *aspeed, bool *inverted)
{
    float rate_offset;
    ahrs_view *_ahrs = get_ahrs_view();

    float bank_angle = _ahrs->roll;

    // limit bank angle between +- 80 deg if right way up
    if (fabsf(bank_angle) < radians(90)) {
        bank_angle = math_constrain_float(bank_angle,-radians(80),radians(80));
        *inverted = false;
    } else {
        *inverted = true;
        if (bank_angle > 0.0f) {
            bank_angle = math_constrain_float(bank_angle,radians(100),radians(180));
        } else {
            bank_angle = math_constrain_float(bank_angle,-radians(180),-radians(100));
        }
    }

    if (!ahrs_airspeed_estimate(_ahrs, aspeed)) {
        // If no airspeed available use average of min and max
        *aspeed = 0.5f*((float)(pitch_ctrl->aparm->airspeed_min) + (float)(pitch_ctrl->aparm->airspeed_max));
    }
    if (abs(_ahrs->pitch_sensor_cd) > 7000) {
        // don't do turn coordination handling when at very high pitch angles
        rate_offset = 0;
    } else {
        rate_offset = cosf(_ahrs->pitch)*fabsf(ToDeg((GRAVITY_MSS / MAX(((*aspeed) * ahrs_get_EAS2TAS(_ahrs)), MAX(pitch_ctrl->aparm->airspeed_min, 1))) * tanf(bank_angle) * sinf(bank_angle))) * pitch_ctrl->_roll_ff;
    }
    if ((*inverted)) {
        rate_offset = -rate_offset;
    }
    return rate_offset;
}

/**
  * @brief       
  * @param[in]   pitch_ctrl  
  * @param[out]  
  * @retval      
  * @note        
  */
static void pctrl_assign_param(mp_pitch_controller_t pitch_ctrl)
{
    param_link_variable(PARAM_ID(PTCH_CTRL, PTCH2SRV_TCONST), &pitch_ctrl->gains.tau);
    param_link_variable(PARAM_ID(PTCH_CTRL, PTCH2SRV_RMAX_UP), &pitch_ctrl->gains.rmax_pos);
    param_link_variable(PARAM_ID(PTCH_CTRL, PTCH2SRV_RMAX_DN), &pitch_ctrl->gains.rmax_neg);
    param_link_variable(PARAM_ID(PTCH_CTRL, PTCH2SRV_RLL), &pitch_ctrl->_roll_ff);

    param_link_variable(PARAM_ID(PTCH_CTRL, PTCH_RATE_P), &pitch_ctrl->rate_pid._kp);
    param_link_variable(PARAM_ID(PTCH_CTRL, PTCH_RATE_I), &pitch_ctrl->rate_pid._ki);
    param_link_variable(PARAM_ID(PTCH_CTRL, PTCH_RATE_D), &pitch_ctrl->rate_pid._kd);
    param_link_variable(PARAM_ID(PTCH_CTRL, PTCH_RATE_FF), &pitch_ctrl->rate_pid._kff);
    param_link_variable(PARAM_ID(PTCH_CTRL, PTCH_RATE_IMAX), &pitch_ctrl->rate_pid._kimax);
    param_link_variable(PARAM_ID(PTCH_CTRL, PTCH_RATE_FLTT), &pitch_ctrl->rate_pid._filt_t_hz);
    param_link_variable(PARAM_ID(PTCH_CTRL, PTCH_RATE_FLTE), &pitch_ctrl->rate_pid._filt_e_hz);
    param_link_variable(PARAM_ID(PTCH_CTRL, PTCH_RATE_FLTD), &pitch_ctrl->rate_pid._filt_d_hz);
    param_link_variable(PARAM_ID(PTCH_CTRL, PTCH_RATE_SMAX), &pitch_ctrl->rate_pid._slew_rate_max);
}

/*------------------------------------test------------------------------------*/


